1. Field of the Invention
Embodiments of the present invention relate to generating optimized loop-closed pose estimates.
2. Related Art
The use of lasers in robotics became popular in the context of estimating the path of a robot equipped with laser sensors, where detecting that the robot has re-visited a known area can be used to decrease the amount of drift that accumulates during pose estimation. Drift occurs due to measurement errors associated with relative pose sensors, e.g., laser, wheel, odometry, and inertial measurement unit data. Global positioning system (GPS) sensors provide absolute pose information that decreases the amount of drift, but GPS information can be itself very inaccurate or missing, especially in urban canyons. The concept of “loop-closing” occurs where it is determined that the robot is at a location that it previously visited whereby that fact can be used to correct estimated positional data.
In a similar manner, loop-closing has also been accomplished using image data to determine location information, also known as structure-from-motion constraints. The laser sensor described above is replaced by a camera, or set of cameras, whereby location data consists of pictures taken by the cameras and a pose is estimated based on information from the camera images. Another method uses image data to estimate the poses of a set of photographs, possibly geo-located, in an area. By calculating the direction from which a picture is taken and combining many different pictures, a composite rendering of a location, e.g., interior or exterior of a building, can be generated. Representative works of this type of image analysis can be found in, for example, Snavely, Seitz, and Szeliski's, “Skeletal Sets for Efficient Structure from Motion,” Proc. Computer Vision and Pattern Recognition (CVPR), 2008, which is an example of generating a structure from motion results for large, unordered, highly redundant, and irregularly sampled photos. An example of a system to match and reconstruct three dimensional scenes from large collections of photographs is further described in, for example, Agarwal, Snavely, Simon, Seitz, and Szeliski's, “Building Rome in a Day,” The Twelfth IEEE International Conference on Computer Vision, (2009).
Vehicles can be equipped with cameras and lasers in order to obtain data for applications such as map making. However, for the gathered data to be useful, the vehicle pose trajectories, or runs, must be estimated accurately. Current state-of-the-art pose estimation typically uses a global positioning system (GPS), an inertial measurement unit (IMU), and wheel encoder sensors to solve for the vehicle pose. However, GPS signals can be inconsistent as the satellite positions are time dependent and can produce large errors where the satellite signal is reflected off of an obstacle, especially in an urban setting. IMU and wheel encoder sensors also have inherent measurement errors. Therefore, the resulting set of pose trajectories from each type of system can be inconsistent, as well as inconsistent with one another.
What is needed is a method for pose estimation using laser or image constraints to make trajectories consistent at intersections.